﻿using System;
using System.Collections.Generic;
using System.Windows.Forms;
using Model01.Exceptions;
using Model01.Management;
using Model01.Modifier;
using Model01.Objects;

namespace Model01.Transportation
{
    public class CityCar : Car
    {
        #region Fields

        private Point destination;
        private Point tmpDest;
        private Point start;
        private double maxWay;
        private double preLeftWay;
        private double ddx;
        private double ddy;
        private double dt;
        private double k = 0.5;
        private double dAngle = 0;
        private double speed;
        private double x0;
        private double y0;
        double R = Manager.ModelLength(4.5);
        private int xTurn;
        private int yTurn;
        private List<Streetlight> streetlights = new List<Streetlight>();
        private bool notLook = false;

        #endregion

        /// <summary>
        ///
        /// </summary>
        /// <param name="x">X coordinate of the car</param>
        /// <param name="y">Y coordinate of the car</param>
        /// <param name="size">Size of the car</param>
        public CityCar(int x, int y, Size size) : base(x, y, size) 
        {
            Start = new Point(x, y);
        }

        #region Methods

        #region Math

        /// <summary>
        /// 
        /// </summary>
        /// <param name="x1">Current point = first point of tangency.</param>
        /// <param name="y1">Current point = first point of tangency.</param>
        /// <param name="x2">Old destination.</param>
        /// <param name="y2">Old destination.</param>
        /// <param name="x3">Destination.</param>
        /// <param name="y3">Destination.</param>
        /// <param name="R">Radius of the circle.</param>
        /// <param name="leftTurn"></param>
        public void CalcCenter(double x1, double y1, double x2, double y2, double x3, double y3, double R, bool leftTurn)
        {
            double k, xed, yed, _xed, _yed;
            if (x2 != x1 && y2 != y1)
            {
                k = (y2 - y1) / (x2 - x1);
                xed = (x2 - x1) / Math.Abs(x2 - x1) * Math.Sqrt(1 / (1 + k * k));
                yed = k * xed;
                if (y1 > y2 == leftTurn)
                    _xed = Math.Sqrt(1 / (1 + (xed * xed) / (yed * yed)));
                else
                    _xed = -Math.Sqrt(1 / (1 + (xed * xed) / (yed * yed)));
                if (x2 > x1 == leftTurn)
                    _yed = Math.Sqrt(1 - _xed * _xed);
                else
                    _yed = -Math.Sqrt(1 - _xed * _xed);
            }
            else
            {
                if (x2 == x1)
                {
                    _yed = xed = 0;
                    yed = (y2 - y1) / Math.Abs(y2 - y1);
                    if (leftTurn)
                        _xed = -yed;
                    else
                        _xed = yed;
                }
                else
                {
                    _xed = yed = 0;
                    xed = (x2 - x1) / Math.Abs(x2 - x1);
                    if (leftTurn)
                        _yed = xed;
                    else
                        _yed = -xed;
                }
            }
            x0 = R * _xed + x1;
            y0 = R * _yed + y1;
            CalcTangencyPoint(x0, y0, x3, y3, x1, y1, R);
        }

        /// <summary>
        /// Calculates the second point of tangency.
        /// </summary>
        /// <param name="x0">Circuls center.</param>
        /// <param name="y0">Circuls center.</param>
        /// <param name="x1">Destination.</param>
        /// <param name="y1">Destination.</param>
        /// <param name="x2">First point of tangency.</param>
        /// <param name="y2">First point of tangency.</param>
        /// <param name="R">Radius of the circle.</param>
        public void CalcTangencyPoint(double x0, double y0, double x1, double y1, double x2, double y2, double R)
        {
            double X = x1 - x0;
            double Y = y1 - y0;
            double a = R * R - Y * Y;
            double b = 2 * X * Y;
            double c = R * R - X * X;
            double k1 = (-b + Math.Sqrt(b * b - 4 * a * c)) / (2 * a);
            double k2 = (-b - Math.Sqrt(b * b - 4 * a * c)) / (2 * a);
            double k = k1;
            a = (k * k + 1);
            b = -2 * (k * (k * y1 - X) + y0);
            double y = -b / (2 * a);
            double _x1 = k1 * (y - y1) + x1;
            double _x2 = k2 * (y - y1) + x1;
            double phi1 = CalcAngle(x2, y2, _x1, y, R);
            double phi2 = CalcAngle(x2, y2, _x2, y, R);
            xTurn = (int)x0;
            yTurn = (int)y0;
            dAngle = phi1;
            Turn(R);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="x1">First point of tangency.</param>
        /// <param name="y1">First point of tangency.</param>
        /// <param name="x2">Second point of tangency.</param>
        /// <param name="y2">Second point of tangency.</param>
        /// <param name="R">Radius of the circle.</param>
        /// <returns></returns>
        public double CalcAngle(double x1, double y1, double x2, double y2, double R)
        {
            return Math.Acos(1 - ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)) / (2 * R * R));
        }
        #endregion

        private void Turn(double R)
        {            
            double alf = speed / R * dt;            
            CurrentAngle += alf;            
            dAngle -= alf;           
            X = xTurn + (int)(R * Math.Cos(Math.PI / 2 - CurrentAngle));
            Y = yTurn - (int)(R * Math.Sin(Math.PI / 2 - CurrentAngle));            
        }

        private int Truncating(ref double value)
        {
            int result = 0;
            if (Math.Abs((int)value) >= 1)
            {
                result = (int)value;
                value = value > result ? value - result : 0;
            }
            return result;
        }

        /// <summary>
        /// Converts double shift to integer.
        /// </summary>
        /// <param name="dShift"></param>
        private void Shift(double dx, double dy)
        {
            ddx += dx;
            ddy += dy;
            this.X += Truncating(ref ddx);
            this.Y += Truncating(ref ddy);
        }

        private void LookStreetlights()
        {
            if (!notLook)
            {
                Point ds;
                if (tmpDest != null)
                    ds = tmpDest;
                else
                    ds = destination;
                //double btwnNearestDest = (ds.X - streetlights[0].X) * (ds.X - streetlights[0].X) + (ds.Y - streetlights[0].Y) * (ds.Y - streetlights[0].Y);
                //double btwnNextDest = (ds.X - streetlights[1].X) * (ds.X - streetlights[1].X) + (ds.Y - streetlights[1].Y) * (ds.Y - streetlights[1].Y);
                double toNearest = (X - streetlights[0].X) * (X - streetlights[0].X) + (Y - streetlights[0].Y) * (Y - streetlights[0].Y);
                double toNext = (X - streetlights[1].X) * (X - streetlights[1].X) + (Y - streetlights[1].Y) * (Y - streetlights[1].Y);
                bool canGo = true;
                if (toNext > toNearest)
                {
                    canGo = !(streetlights[0].CurrentLight == Lights.Yellow && streetlights[0].NextLight == Lights.Red || streetlights[0].CurrentLight == Lights.Red);
                }
                else
                    notLook = true;
                if (!canGo)
                {
                    if (tmpDest == null)
                    {
                        tmpDest = destination;
                        destination = streetlights[0];
                        GoTo(destination);
                    }
                }
                else
                {
                    if (tmpDest != null)
                    {
                        destination = tmpDest;
                        tmpDest = null;                        
                        GoTo(destination);
                    }
                }
            }
            else
                return;
        }

        private void GetStreetlights()
        {
            Streetlight nearestSl = Manager.streetlights[0];
            Streetlight nextSl = null;
            int minDist = ((X - Manager.streetlights[0].X) * (X - Manager.streetlights[0].X) + (Y - Manager.streetlights[0].Y) * (Y - Manager.streetlights[0].Y));
            int minIndex = 0;
            for (int i = 1; i < Manager.streetlights.Count; i++)
            {
                int dist = ((X - Manager.streetlights[i].X) * (X - Manager.streetlights[i].X) + (Y - Manager.streetlights[i].Y) * (Y - Manager.streetlights[i].Y));
                if (dist < minDist)
                {
                    minDist = dist;
                    nearestSl = Manager.streetlights[i];
                    minIndex = i;
                }
            }
            for (int i = 0; i < Manager.streetlights.Count; i++)
            {
                if (i != minIndex)
                {                    
                    nextSl = Manager.streetlights[i];
                    double kSlts = (double)(Manager.streetlights[i].Y - nearestSl.Y) / (double)(Manager.streetlights[i].X - nearestSl.X);
                    double kRoute = (double)(destination.Y - Y) / (double)(destination.X - X);
                    if (Math.Round(kSlts - kRoute, 2) == 0 || destination.X == X && Manager.streetlights[i].X == nearestSl.X)
                    {
                        streetlights.Add(nearestSl);
                        streetlights.Add(nextSl);
                        return;
                    }
                }
            }           
        }

        protected override void OnCarStep(object sender, System.Timers.ElapsedEventArgs e)
        {
            double ds;
            if (dAngle > 0)
            {
                #region Turning                               
                double rotSpeed = Math.Sqrt(k * R * Manager.g);
                if (speed > rotSpeed)
                {
                    ds = Manager.AccelerateCoefficient * (speed - maxDeceleration * dt / 2) * dt;
                    speed -= maxDeceleration * dt;
                    Shift(ds * Math.Cos(CurrentAngle), ds * Math.Sin(CurrentAngle));
                }
                else
                    Turn(R);
                #endregion
            }
            else
            {                
                #region Derect Movement   
                LookStreetlights();
                int leftWay = (int)Math.Sqrt((destination.X - this.X) * (destination.X - this.X) + (destination.Y - this.Y) * (destination.Y - this.Y));
                if (preLeftWay >= leftWay)
                {
                    preLeftWay = leftWay;
                    double brakingDistanceFirst = speed * speed / (2 * minDeceleration);
                    double brakingDistanceSecond = speed * speed / (2 * deceleration);
                    if (Math.Abs((int)leftWay) > 1)
                    {
                        if (brakingDistanceFirst <= leftWay - maxWay)
                        {
                            if (speed < Manager.MaxSpeed)
                            {
                                speed += acceleration * dt;
                                ds = Manager.AccelerateCoefficient * (speed + acceleration * dt / 2) * dt;
                            }
                            else
                            {
                                speed = Manager.MaxSpeed;
                                ds = maxWay;
                            }
                            Shift(ds * Math.Cos(CurrentAngle), ds * Math.Sin(CurrentAngle));
                        }
                        else
                        {
                            if (brakingDistanceSecond <= leftWay)
                            {
                                ds = Manager.AccelerateCoefficient * (speed - minDeceleration * dt / 2) * dt;
                                speed -= minDeceleration * dt;
                            }
                            else
                            {
                                ds = Manager.AccelerateCoefficient * (speed - maxDeceleration * dt / 2) * dt;
                                speed -= maxDeceleration * dt;
                            }
                        }
                        Shift(ds * Math.Cos(CurrentAngle), ds * Math.Sin(CurrentAngle));
                    }
                }
                #endregion
            }
        }

        /// <summary>
        /// Makes a car go to it's destination point.
        /// </summary>
        /// <param name="destination">Model01.Objects.Point where does the car go.</param>
        protected override void GoTo(Point destination)
        {
            dt = (int)Manager.TimeRepaint * 0.001;
            maxWay = Manager.MaxSpeed * dt;
            preLeftWay = (int)Math.Sqrt((destination.X - this.X) * (destination.X - this.X) + (destination.Y - this.Y) * (destination.Y - this.Y));
            GetStreetlights();
            timer.Start();
        }
        
        /// <summary>
        /// Makes a car go to it's route.
        /// </summary>
        /// <exception cref="Model01.Exceptions.NullDestinationException"/>
        public override void Go()
        {
            if (this.destination != null)
                GoTo(this.destination);
            else
                throw new NullDestinationException();
        }

        #endregion

        #region Properties

        /// <summary>
        /// Gets or sets start point.
        /// </summary>
        public Point Start
        {
            get { return start; }
            set { start = value; }
        }

        /// <summary>
        /// Gets or sets destination point.
        /// </summary>
        public Point Destination
        {
            get { return destination; }
            set 
            {
                if (destination != null)
                {
                    CalcCenter(X, Y, destination.X, destination.Y, value.X, value.Y, R, true);
                    destination = value;
                }
                else
                    destination = value;
            }
        }

        #endregion

        
    }
}
